/*
 *  body.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 15/11/07.
 *
 */

#include "body.h"

Body::Body()
:	name(""),
	dim(3),
	parentSystem(0),
	vComponents(0),
	vLoads(0),
	vPointNamesSurface(),
	vPPointSurface(),
	pNamePNumber(),
	outGraphFilex(),
	localFGraph(),
	contactAllOption(false),
	contactFileOption(false),
	contactNamesOption(false)
{}

// ***

Body::Body(ifstream &in, string cname, MecSystem* pSys)
:	name(cname),
	dim(3),
	parentSystem(pSys),
	vComponents(0),
	vLoads(0),
	vPointNamesSurface(),
	vPPointSurface(),
	pNamePNumber(),
	outGraphFilex(),
	localFGraph(),
	contactAllOption(false),
	contactFileOption(false),
	contactNamesOption(false)
{}

// ***

void Body::reportContent()
{
	int i;
	for(i=0; i<vComponents.size(); i++)
	{
		cout << "			Component " << i << " with points \n";
		vComponents[i]->reportContent();
	}
	for(i=0; i<vLoads.size(); i++)
	{
		cout << "			Load " << i << " with points \n";
		vLoads[i]->reportContent();
	}

}

// ***

void Body::setDofs()
{
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->setDofs();
	for(int i=0; i<vLoads.size(); i++)	vLoads[i]->setDofs();
}

// ***

void Body::setLocalGlob()
{
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->setLocalGlob();
	for(int i=0; i<vLoads.size(); i++)	vLoads[i]->setLocalGlob();
}

// ***

void Body::setLoad(string lType, ifstream &inn, const vector<double> &vTime, const map<string,vector<string>,less<string> >* pset) // edit
{
	if(!lType.compare("concentrated"))
	{
		vLoads.push_back(new CLoad(inn,name,dim, pset) );
	}
	if(!lType.compare("concentrated-t"))
	{
		vLoads.push_back(new CTLoad(inn,name,dim,vTime, pset) );
	}
	else if(!lType.compare("interactive"))
	{
		vLoads.push_back(new IACLoad(inn,name,dim, pset) );
	}
}

// ***

void Body::reportLoads()
{
	for(int i=0;i<vLoads.size();i++)
	{
		for(int k=0;k< (vLoads[i]->pointNames.size()); k++)
		{
			cout << "Point *********" << vLoads[i]->pointNames[k] << "\n";
		}
	}
}

// ***

void Body::initializeGlobalDofs(vector<double> &q, vector<double> &dq, double & time)
{
	int i;
	for(i=0; i<vComponents.size(); i++) vComponents[i]->initializeGlobalDofs(q,dq,time);
	for(i=0; i<vLoads.size(); i++) vLoads[i]->initializeGlobalDofs(q,dq,time);
}

// ***

void Body::fillResidual(vector<double>& res_in, vector<double>& q_in, vector<double>& dq_in, const double& time, const int& modeFlag, const double& grav, const int& gravDir, const double& DeltaT)
{
	int i;
	// Last two 0 are related to Augmented Lagrangian. TO BE DONE
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->fillResidual(res_in, q_in, dq_in, time, modeFlag,grav, gravDir, DeltaT);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->fillResidual(res_in,q_in, dq_in, time, modeFlag, grav, gravDir, DeltaT);
}

// ***

void Body::fillResidual(	vector<double>& residue,
							const vector<double>& q,
							const vector<double>& qdot,
							const vector<double>& qddot,
							const double& time,
							const int& modeFlag,
							const double& grav, const int& gravDir,
//JC							const int& integratorType, const vector<double>& qConv, const vector<double>& dqConv,
							const int& integratorType,
							const double& DeltaT
						)
{
	int i;
/*JC
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->fillResidual(residue,q,qdot,qddot,time, modeFlag, grav, gravDir,integratorType, qConv, dqConv, DeltaT);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->fillResidual(residue,q,qdot,qddot,time, modeFlag, grav, gravDir,integratorType, qConv, dqConv, DeltaT);
*/
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->fillResidual(residue,q,qdot,qddot,time, modeFlag, grav, gravDir,integratorType, DeltaT);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->fillResidual(residue,q,qdot,qddot,time, modeFlag, grav, gravDir,integratorType, DeltaT);
}

// ***

void Body::fillResFi(	vector<double>& residue,
						  const vector<double>& q,
						  const vector<double>& qddot,
						  const int& modeFlag
						  )
{
	int i;
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->fillResFi(residue,q,qddot, modeFlag);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->fillResFi(residue,q,qddot, modeFlag);	
}

// ***

void Body::fillResFq(	vector<double>& residue,
						  const vector<double>& q,
						  const vector<double>& qdot,
						  const int& modeFlag,
//JC						  const int& integratorType, const vector<double>& qConv, const vector<double>& dqConv,
						  const int& integratorType,
						  const double& DeltaT
						  )
{
	int i;
/*
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->fillResFq(residue,q,qdot,modeFlag, integratorType, qConv, dqConv, DeltaT);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->fillResFq(residue,q,qdot,modeFlag, integratorType, qConv, dqConv, DeltaT);
*/
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->fillResFq(residue,q,qdot,modeFlag, integratorType, DeltaT);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->fillResFq(residue,q,qdot,modeFlag, integratorType, DeltaT);
}

// ***

void Body::fillResFt(	vector<double>& residue,
						  const double& time,
						  const int& modeFlag,
						  const double& grav, const int& gravDir
						  )
{
	int i;
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->fillResFt(residue,time, modeFlag, grav, gravDir);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->fillResFt(residue,time, modeFlag, grav, gravDir);	
}

// ***
/*
void Body::fillTangent(sparseMatrix& jac_in, vector<double>& q_in, const double& time, const int& modeFlag)
{
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->fillTangent(jac_in,q_in, time, modeFlag);
	for(int i=0; i<vLoads.size(); i++)	vLoads[i]->fillTangent(jac_in,q_in, time, modeFlag);
}
*/
// ***

void Body::fillTangent(sparseMatrix& Kint, vector<vector<double> >& Kbb, vector<vector<double> >& Kib, vector<vector<double> >& Kbi, 
					   vector<int>& globalLocalI, vector<int>& globalLocalB, 
					   vector<double>& q_in, vector<double>& dq_in,
					   const double& time, const int& modeFlag)
{
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->fillTangent(Kint, Kbb, Kib, Kbi, globalLocalI, globalLocalB, q_in, dq_in, time, modeFlag);
	for(int i=0; i<vLoads.size(); i++)	vLoads[i]->fillTangent(Kint, Kbb, Kib, Kbi, globalLocalI, globalLocalB, q_in, dq_in, time, modeFlag);
}

// ***
/*
void Body::fillTangent	(	sparseMatrix& tangent,
							const vector<double>& q,
							const vector<double>& qdot,
							const vector<double>& qddot,
							double partial_qdot,
							double partial_qddot,
							const double& time,
							const int& modeFlag,
 							const int& integratorType,
							const double& DeltaT
						)
{

	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->fillTangent(tangent,q,qdot,qddot,partial_qdot,partial_qddot, time, modeFlag,integratorType, DeltaT);
	for(int i=0; i<vLoads.size(); i++)	vLoads[i]->fillTangent(tangent,q,qdot,qddot,partial_qdot,partial_qddot,time,modeFlag,integratorType, DeltaT);
}
*/

// ***

void Body::fillTangent	(sparseMatrix& Kint, vector<vector<double> >& Kbb, vector<vector<double> >& Kib, vector<vector<double> >& Kbi,
						 vector<int>& globalLocalI, vector<int>& globalLocalB,	
						 const vector<double>& q,
						 const vector<double>& qdot,
						 const vector<double>& qddot,
						 double partial_qdot,
						 double partial_qddot,
						 const double& time,
						 const int& modeFlag,
						 const int& integratorType,
						 const double& DeltaT
						 )
{
 
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->fillTangent(Kint, Kbb, Kib, Kbi, globalLocalI, globalLocalB,q,qdot,qddot,partial_qdot,partial_qddot, time, modeFlag,integratorType, DeltaT);
	for(int i=0; i<vLoads.size(); i++)	vLoads[i]->fillTangent(Kint, Kbb, Kib, Kbi, globalLocalI, globalLocalB, q,qdot,qddot,partial_qdot,partial_qddot,time,modeFlag,integratorType, DeltaT);
}
 
// ***

void Body::fillMassMatrix(sparseMatrix& Ma, const vector<double>& q, const double& time)
{
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->fillMassMatrix(Ma,q,time);
}

// ***

void Body::fillMassMatrix(vector<double>& vMa, const vector<double>& q, const double& time)
{
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->fillMassMatrix(vMa,q,time);
}

// ***

void Body::fillForce(vector<double>& f,
						const vector<double>& q,
						const vector<double>& qdot,
						const double& time,
						const double& grav, const int& gravDir
					)
{
	int i;
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->fillForce(f, q, qdot, time, grav, gravDir);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->fillForce(f, q, qdot, time, grav, gravDir);
}

// ***

void Body::setSparsePattern(vector< vector<int> >& tempIa)
{
	int i;
//		cout << "vComponents.size en Body " << vComponents.size() << endl;
//		cout << "vLoads.size en Body " << vLoads.size() << endl;
	for(i=0; i<vComponents.size(); i++) vComponents[i]->setSparsePattern(tempIa);
	for(i=0; i<vLoads.size(); i++) vLoads[i]->setSparsePattern(tempIa);
}

// ***

void Body::setSparsePatternInertia(vector< vector<int> >& tempIa)
{
	int i;
	//		cout << "vComponents.size en Body " << vComponents.size() << endl;
	//		cout << "vLoads.size en Body " << vLoads.size() << endl;
	for(i=0; i<vComponents.size(); i++) vComponents[i]->setSparsePatternInertia(tempIa);
	for(i=0; i<vLoads.size(); i++) vLoads[i]->setSparsePatternInertia(tempIa);
}

// ***

void Body::changePointName(string& oldName, string& newName)
{
	int i;
	for(i=0; i<vComponents.size(); i++) vComponents[i]->changePointName(oldName,newName);
	for(i=0; i<vLoads.size(); i++) vLoads[i]->changePointName(oldName,newName);
}	

// ***

void Body::writeFileGraph(ofstream& fGraph, string& gFormat, string& gPath, string& aux, bool& plotDispFlag, bool& plotProcFlag, bool& plotStressFlag)
{
	if (!(gFormat.compare("msh")))	writeMSH(fGraph);
	if (!(gFormat.compare("vtu")))
	{
		if(pNamePNumber.size()==0)
		{
			int i;
			// Fill table pointName - pointNumber of the surface points
			for(i=0; i<vPointNamesSurface.size(); ++i)
			{
				pNamePNumber[ vPointNamesSurface[i] ] = i;
			}		
		}
		// Define the localFGraph stream for the body data
		// The format of the filename is systemname-bodyname-proc-numStep.vtu
		string isini;
		outGraphFilex.clear();
        outGraphFilex = gPath + "Graph";
		outGraphFilex+= aux.substr(0,aux.find_last_of("-"));
		outGraphFilex+= "-parts/";
		outGraphFilex+= name;
		outGraphFilex = subsSlash(outGraphFilex);
		outGraphFilex+= "-";
		outGraphFilex+= aux;
		outGraphFilex+= ".vtu";
		
		char *caux = new char [outGraphFilex.size()+1];
		strcpy (caux, outGraphFilex.c_str());
		
		localFGraph.open(caux);
		writeVTU(fGraph, plotDispFlag, plotProcFlag,plotStressFlag);
		localFGraph.close();
		
		// Write the line in the .pvd file
		fGraph << "		<DataSet timestep=\"";
		fGraph << aux.substr(aux.find_last_of("-")+1);
		fGraph << "\" group=\"\" part=\"";
		fGraph << aux.substr(0,aux.find_last_of("-"));
		fGraph << "\" file=\"";
		fGraph << outGraphFilex;
		fGraph << "\"/>\n";		
	}
	if (!(gFormat.compare("oog")))	writeOOGL(fGraph);
}

// ***

void Body::initializeCompRes(const bool& st, const double& grav, const int& gravDir)
{
	int i;
	// 0 is related to augmented Lagrangian. It does not affect bodies, just joints
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->initializeCompRes(st,grav,gravDir,0);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->initializeCompRes(st,grav,gravDir,0);
}

// ***

//void Body::initializeCompTang(const bool& st, const bool& lmf)
void Body::initializeCompTang(const bool& st)
{
	int i;
	// 0 related to constraint projection, which is not used in bodies.
	/*
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->initializeCompTang(st,lmf);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->initializeCompTang(st,lmf);
	*/
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->initializeCompTang(st);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->initializeCompTang(st);
}

// ***

void Body::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q, vector<double>& dq)
{
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->calcEner(Ekin, Epot, Econ, Eext, Eamb, Dint, grav, gravDir, q, dq);
	for(int i=0; i<vLoads.size(); i++)	vLoads[i]->calcEner(Ekin, Epot, Econ, Eext, Eamb, Dint, grav, gravDir, q, dq);
}

// ***

void Body::calcEntropy(double& Entropy, double& EntropyAmb)
{
	int i;
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->calcEntropy(Entropy,EntropyAmb);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->calcEntropy(Entropy,EntropyAmb);
}

// ***

void Body::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{
	int i;
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->calcAM(am, q_in, dq_in);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->calcAM(am, q_in, dq_in);
}

// ***

void Body::update(const int& integratorType, const vector<double>& qConv, const vector<double>& dqConv)
{
	int i;		
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->update(integratorType, qConv, dqConv);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->update(integratorType, qConv, dqConv);
}

// ***

void Body::setInitialConditions(vector<double>& q, vector<double>& dq)
{
	int i;
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->setInitialConditions(q,dq);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->setInitialConditions(q,dq);
}

// ***

void Body::setTemperature(double& theta)
{
	int i;
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->setTemperature(theta);
	for(i=0; i<vLoads.size(); i++)	vLoads[i]->setTemperature(theta);
}

// ***

int Body::setTRNumbers(int num)
{
	int i;
	for(i=0; i<vComponents.size(); i++)	num = vComponents[i]->setTRNumbers(num);
	for(i=0; i<vLoads.size(); i++)		num = vLoads[i]->setTRNumbers(num);
	
	return num; 
}	

// ***

void Body::updateDofs(vector<double>& q, vector<double>& deltaq)
{
	int i;

	for(i=0; i<vComponents.size(); i++)	vComponents[i]->updateDofs(q,deltaq);
	for(i=0; i<vLoads.size(); i++)		vLoads[i]->updateDofs(q,deltaq);
	
}

// ***

int Body::setInertiaNumbers(int num, vector<int>& v)
{
	int i;
	
	for(i=0; i<vComponents.size(); i++)	num = vComponents[i]->setInertiaNumbers(num,v);
	for(i=0; i<vLoads.size(); i++)		num = vLoads[i]->setInertiaNumbers(num,v);	
	return num;
}

// ***

void Body::setQatComponents(const vector<double>& q, const vector<double>& dq)
{
	int i;
	
	for(i=0; i<vComponents.size(); i++)	vComponents[i]->getLocalDofs(q,dq);
	for(i=0; i<vLoads.size(); i++)		vLoads[i]->getLocalDofs(q,dq);			
}

// ***

void Body::getPointNames(vector<string>& pNames)
{
	int i, j;
	vector<string> auxs;
	
	for(i=0; i<vComponents.size(); i++)
	{
		auxs.clear();
		auxs = vComponents[i]->getPointNames();
		for (j=0; j<auxs.size(); ++j)	pNames.push_back(auxs[j]);
	}
}

// ***

void Body::fillMSmooth(sparseMatrix& Ma, const vector<double>& q, const double& time)
{
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->fillMSmooth(Ma,q,time);
}

// ***

void Body::fillFSmooth(vector<vector<double> >& Fs, const vector<double>& q_in, const vector<double>& dq_in, const double& time)
{
	for(int i=0; i<vComponents.size(); i++)	vComponents[i]->fillFSmooth(Fs,q_in,dq_in,time);
}

int Body::checkComponents(int state)
{
	for(int i=0; i<vComponents.size(); i++)	state = vComponents[i]->checkComponents(state);
	//for(i=0; i<vLoads.size(); i++)	vLoads[i]->update(integratorType, qConv, dqConv);
	//cout << "body:state is " << state << endl;
	return state;
}

